#ifndef RobotArm_H
#define RobotArm_H

#include <QObject>
#include <QVariantMap>
#include "RobotCmdNode.hpp"
#include "types.h"

class QTcpSocket;
class QSharedMemory;


class RobotArm : public QObject
{
	Q_OBJECT
public:
	explicit RobotArm(int argc, char** argv, QObject* parent = nullptr);
	~RobotArm();

	void connect2Host(const std::string& strIp, unsigned short usPort);

	void setLeftEnable(quint16 e);
	void setRightEnable(quint16 e);
	void setLeftControlMode(quint16 e);
	void setRightControlMode(quint16 e);
	void setLeftErrClean(quint16 e);
	void setRightErrClean(quint16 e);
	void setLeftFTMode(quint16 e);
	void setRightFTMode(quint16 e);
	void setLeftSecMode(quint16 e);
	void setRightSecMode(quint16 e);

	void call_writeDuoArmDirect(const QVariantMap& leftCmd, const QVariantMap& rightCmd);

private slots:
	void connect2Host_p();
	void onSocketConnected();
	void onSocketReadReady();
    void writeDuoArmDirect(const QVariantMap& leftCmd, const QVariantMap& rightCmd);

private:
	void rosSpinThread();
	int parse(quint8* buf, int len);


private:
	QTcpSocket* m_socket;
	int argc_;
	char** argv_;
	std::shared_ptr<RobotCmdNode> m_robotCmdNode;
	std::thread m_ros_spin_thread;

	QString m_strIp;
	quint16 m_usPort;

	// parmas
	quint16 m_usEnable;
	quint16 m_usControlMode;
	quint16 m_usErrClean;
	quint16 m_usFTMode;
	quint16 m_usSecMode;

	quint16 m_usLeftEnable;
	quint16 m_usRightEnable;
	quint16 m_usLeftControlMode;
	quint16 m_usRightControlMode;
	quint16 m_usLeftErrClean;
	quint16 m_usRightErrClean;
	quint16 m_usLeftFTMode;
	quint16 m_usRightFTMode;
	quint16 m_usLeftSecMode;
	quint16 m_usRightSecMode;

	QSharedMemory* m_smTcp;
};

#endif // RobotArm_H
